/* Astar implementation
 *
 * Astar implementation, currently implementing using a heap provided by
 * the standard stl heap functions in algorithm.
 *
 * By: M. Etheredge
 *     S. Brattinga
 */

#include "astar.h"

#include <stdio.h>

using namespace std;

Astar::Node ***Astar::g_Map;

int Astar::g_Width;
int Astar::g_Height;

int Astar::g_State;

Astar::Node *Astar::g_AdjacentNodes[8];

void Astar::InitializeMap( vector<Node*> &p_Nodes, const int p_Width )
{
    int x, y, i;

    const int height = p_Nodes.size() / p_Width;

    srand( time( NULL ) );

    if( p_Width == 0 || height == 0 )
    {
        return;
    }

    g_Height = height;
    g_Width  = p_Width;

    g_Map = (Node***) malloc( g_Height * sizeof( Node** ) );

    for( y = 0 ; y < g_Height ; ++y )
    {
        g_Map[y] = (Node**) malloc( g_Width * sizeof( Node* ) );

        for( x = 0 ; x < g_Width ; ++x )
        {
        	i = y * g_Height + x;

            g_Map[y][x] = p_Nodes.at( i );
            g_Map[y][x]->SetX( x );
            g_Map[y][x]->SetY( y );
            //g_Map[y][x]->SetHeight( ((sin( y ) + cos( x )) * (rand() % 20)) );
            g_Map[y][x]->SetHeight(rand() % 10);

            if( rand() % 10 > 8 )
            {
                g_Map[y][x]->SetObstacle( true );
            }
            else
            {
                g_Map[y][x]->SetObstacle( false );
            }
        }
    }

    g_State = STATE_INITIALIZED_MAP;
}

void Astar::SetAdjacentNodeIndices( Node *p_Node )
{
    int x = p_Node->GetX(), y = p_Node->GetY();

    g_AdjacentNodes[ADJACENT_POSITION_TOP_LEFT ] = (x - 1 > 0 && y - 1 > 0              ? g_Map[y - 1][x - 1] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_TOP      ] = (y - 1 > 0                           ? g_Map[y - 1][x    ] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_TOP_RIGHT] = (x + 1 < g_Width && y - 1 > 0        ? g_Map[y - 1][x + 1] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_LEFT     ] = (x - 1 > 0                           ? g_Map[y    ][x - 1] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_RIGHT    ] = (x + 1 < g_Width                     ? g_Map[y    ][x + 1] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_BOT_LEFT ] = (x - 1 > 0 && y + 1 < g_Height       ? g_Map[y + 1][x - 1] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_BOT      ] = (y + 1 < g_Height                    ? g_Map[y + 1][x    ] : 0);
    g_AdjacentNodes[ADJACENT_POSITION_BOT_RIGHT] = (x + 1 < g_Width && y + 1 < g_Height ? g_Map[y + 1][x + 1] : 0);
}

void Astar::FreeMap( void )
{
    int i = 0;

    for( i = 0 ; i < g_Height ; ++i )
    {
        free( g_Map[i] );
    }

    free( g_Map );

    g_Width  = 0;
    g_Height = 0;

    g_State = STATE_UNINITIALIZED_MAP;
}

list<Astar::Node*> Astar::Astar( Node *p_Start, Node *p_Goal )
{
    Node *current, *adjacentNode;

    int i;

    vector<Node*> openList;
    list<Node*> closedList;

    if( g_State == STATE_UNINITIALIZED_MAP )
    {
        return closedList;
    }

    // Push back the start to the open list
    openList.push_back( p_Start );

    while( !openList.empty() )
    {
    	current = openList.front(); openList.erase( openList.begin() ); // Pop the front
    	openList.clear(); 					    						// Clear the open list

        if( current == p_Goal ) // Reached the goal
        {
            break;
        }
        else                    // Astar logic
        {
            closedList.push_back( current );

            SetAdjacentNodeIndices( current );

            make_heap( openList.begin(), openList.end(), HeapCompareCost() );

            for( i = 0 ; i < 8 ; ++i )
            {
                adjacentNode = g_AdjacentNodes[i];

                if( adjacentNode != 0 )
                {
                    // Set the cost of diagonal adjacent nodes to 14
                    if( i == ADJACENT_POSITION_TOP_LEFT || i == ADJACENT_POSITION_TOP_RIGHT || i == ADJACENT_POSITION_BOT_LEFT || i == ADJACENT_POSITION_BOT_RIGHT )
                    {
                        adjacentNode->SetCost( 14 + adjacentNode->GetHeight() );
                    }
                    else
                    {
                        adjacentNode->SetCost( 10 + adjacentNode->GetHeight() );
                    }

                    // Set the cumulative cost, used by the heap compare cost function
                    adjacentNode->SetCumulativeCost( Heuristics::Score( 10, p_Goal, adjacentNode ) );

                    // This will be kind of slow
                    if( (find( openList.begin(), openList.end(), adjacentNode ) == openList.end()) &&
                    	(find( closedList.begin(), closedList.end(), adjacentNode ) == closedList.end()))
                    {
                        if(!adjacentNode->GetObstacle())
                        {
                            openList.push_back( adjacentNode ); // Push the adjacent node to the back of the vector
                        }
                        // Run a push sweep over the heap to make sure that the last pushed element is actually in a valid place on the heap
                        push_heap( openList.begin(), openList.end(), HeapCompareCost() );
                    }
                }
            }
        }
    }

    // Free the map
    FreeMap();

    return closedList;
}

int Heuristics::Score( const int p_MinimumCost, Astar::Node *p_Goal, Astar::Node *p_Current )
{
    return p_Current->GetCost() + ManhattanDistance( p_MinimumCost, p_Goal, p_Current );
}

int Heuristics::ManhattanDistance( const int p_MinimumCost, Astar::Node *p_Goal, Astar::Node *p_Current )
{
    return p_MinimumCost * (abs( p_Current->GetX() - p_Goal->GetX() ) + abs( p_Current->GetY() - p_Goal->GetY() ));
}
